#!/usr/bin/env python


_prefix = "Path"


import rospy
from move_base_msgs.msg import *

from bt_tree_lib.bt_tree_ros import *
from GenGoal import *


Path_SubTree_Dict = {}


class Path_SubTreeParamStatus(object):
    PREPARING = 0
    EXECUTING = 1


class Path_SubTreeParam(object):
    def __init__(self, *args, **kwargs):
        super(Path_SubTreeParam, self).__init__(*args, **kwargs)
        self.status = Path_SubTreeParamStatus.PREPARING

        self.goal = MoveBaseGoal()
        return

    def get_status(self):
        return self.status

    def set_status(self, status):
        self.status = status
        return

    def reset(self):
        self.status = Path_SubTreeParamStatus.PREPARING

        self.__set_goal(self.goal, MoveBaseGoal())
        return

    def set(self, param):
        self.__set_goal(self.goal, param)
        return

    def __set_goal(self, goal, param):
        goal.target_pose = param.target_pose
        return


def Path_init():
    gen_goal()
    return


def Path_cancel():
    for key, value in Path_SubTree_Dict.items():
        value['SubTreeParam'].reset()
        value['SubTree'].reset()
    return


def Path_get_SubTree(name):
    # Input check.
    if not isinstance(name, str):
        return False, None, None, "Parameter passed is not a string."
    if Path_SubTree_Dict.has_key(name):
        return False, None, None, "The Path_SubTree %s has already existed." % name
    
    SubTreeParam = Path_SubTreeParam()
    SubTree = _Path_get_SubTree(SubTreeParam)
    
    # Save the SubTree, and return result.
    Path_SubTree_Dict[name] = {'SubTreeParam': SubTreeParam, 'SubTree': SubTree}
    return True, SubTreeParam, SubTree, ""


def _Path_get_SubTree(param):
    HAS_PATH_EXECUTING = PathNode_has_Path_EXECUTING("HAS_PATH_EXECUTING", param)
    GET_A_NEW_GOAL = Get_A_New_Goal("GET_A_NEW_GOAL", param)
    PATH_LOOP_HEADER = Selector("PATH_LOOP_HEADER", [HAS_PATH_EXECUTING, GET_A_NEW_GOAL])
    PATH_LOOP_HEADER_WRAPPER = DecoratorNode_NOT("PATH_LOOP_HEADER_WRAPPER", PATH_LOOP_HEADER)

    SET_PATH_EXECUTING = PathNode_set_Path_EXECUTING("SET_PATH_EXECUTING", param)
    NAVIGATE = SimpleActionNode("NAVIGATE", "move_base", MoveBaseAction, param.goal, 10)
    SET_PATH_PREPARING = PathNode_set_Path_PREPARING("SET_PATH_PREPARING", param)
    PATH_LOOP_BODY = Sequence_withMemory("PATH_LOOP_BODY", [SET_PATH_EXECUTING, NAVIGATE, SET_PATH_PREPARING])


    return Selector("PATH_LOOP", [PATH_LOOP_HEADER_WRAPPER, PATH_LOOP_BODY])


class PathNode_has_Path_EXECUTING(LeafNode):
    def __init__(self, name, param, *args, **kwargs):
        super(PathNode_has_Path_EXECUTING, self).__init__(name, *args, **kwargs)
        self.param = param
        return

    def run(self):
        status = self.param.get_status()
        if status == Path_SubTreeParamStatus.EXECUTING:
            return NodeStatus.SUCCESS
        return NodeStatus.FAILURE


class PathNode_set_Path_EXECUTING(LeafNode):
    def __init__(self, name, param, *args, **kwargs):
        super(PathNode_set_Path_EXECUTING, self).__init__(name, *args, **kwargs)
        self.param = param
        return

    def run(self):
        self.param.set_status(Path_SubTreeParamStatus.EXECUTING)
        return NodeStatus.SUCCESS


class PathNode_set_Path_PREPARING(LeafNode):
    def __init__(self, name, param, *args, **kwargs):
        super(PathNode_set_Path_PREPARING, self).__init__(name, *args, **kwargs)
        self.param = param
        return

    def run(self):
        self.param.set_status(Path_SubTreeParamStatus.PREPARING)
        return NodeStatus.SUCCESS


class Get_A_New_Goal(LeafNode):
    def __init__(self, name, param, *args, **kwargs):
        super(Get_A_New_Goal, self).__init__(name, *args, **kwargs)
        self.param = param
        return

    def run(self):
        flag, goal = get_goal()
        if flag:
            self.param.set(goal)
            return NodeStatus.SUCCESS
        return NodeStatus.FAILURE



